#!/bin/bash

echo "=== 优化版点云建图系统启动 ==="

# 检查ROS 2环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: 未找到ROS 2环境，请先source ROS 2环境"
    echo "运行: source /opt/ros/humble/setup.bash"
    exit 1
fi

# 检查NumPy版本
echo "检查NumPy版本..."
python3 -c "
import numpy as np
version = np.__version__
print(f'当前NumPy版本: {version}')
if version.startswith('2'):
    print('错误: NumPy版本为2.x，需要降级到1.x版本')
    print('请运行: pip3 install numpy==1.24.3')
    exit(1)
else:
    print('✅ NumPy版本兼容')
"

# 1. 启动RealSense相机
echo "步骤1: 启动RealSense相机..."
gnome-terminal --title="RealSense Camera" -- bash -c "
echo '启动RealSense相机节点...';
source /opt/ros/humble/setup.bash;
ros2 launch realsense2_camera rs_launch.py \
    pointcloud.enable:=true \
    align_depth.enable:=true \
    enable_color:=true \
    enable_depth:=true \
    depth_module.depth_profile:=848x480x30 \
    rgb_camera.color_profile:=1280x720x30;
echo 'RealSense节点已停止';
read -p '按Enter键关闭窗口...'
"

sleep 8

# 2. 启动RTAB-Map建图
echo "步骤2: 启动RTAB-Map建图系统..."
gnome-terminal --title="RTAB-Map SLAM" -- bash -c "
echo '启动RTAB-Map建图节点...';
source /opt/ros/humble/setup.bash;
ros2 launch rtabmap_launch rtabmap.launch.py \
    rtabmap_args:='--delete_db_on_start' \
    frame_id:=camera_link \
    rgb_topic:=/camera/camera/color/image_raw \
    depth_topic:=/camera/camera/depth/image_rect_raw \
    camera_info_topic:=/camera/camera/color/camera_info \
    approx_sync:=true \
    rtabmap_viz:=true \
    rviz:=true;
echo 'RTAB-Map节点已停止';
read -p '按Enter键关闭窗口...'
"

sleep 8

# 3. 启动点云保存服务
echo "步骤3: 启动点云保存服务..."
gnome-terminal --title="PointCloud Saver" -- bash -c "
echo '启动点云保存节点...';
cd ~/shell && python3 save_pointcloud.py;
echo '点云保存节点已停止';
read -p '按Enter键关闭窗口...'
"

echo ""
echo "=== 所有系统已启动完成 ==="
echo ""
echo "✅ 系统状态:"
echo "   - RealSense相机: 运行中"
echo "   - RTAB-Map建图: 运行中" 
echo "   - 点云保存服务: 运行中"
echo ""
echo "📝 操作指南:"
echo "1. 在RViz中查看实时建图效果"
echo "2. 手持相机缓慢绕室内行走进行建图"
echo "3. 建图完成后，在新的终端中运行保存命令:"
echo "   ros2 service call /save_pointcloud std_srvs/srv/Trigger"
echo ""
echo "💾 保存位置:"
echo "   点云文件将保存到: ~/pointcloud_maps/"
echo "   查看保存的文件: ls -la ~/pointcloud_maps/"
echo ""
echo "🛠️  故障排除:"
echo "   如果点云保存失败，确保NumPy版本正确: pip3 install numpy==1.24.3"
